
#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "basic_sprayer_interfaces/msg/adc.hpp"   //adc的Message
#include "sensor_msgs/msg/imu.hpp"    
#include <iio.h>

#include <signal.h>
//#include <DspFilters/Butterworth.h>
#include "imu.hpp"


using namespace std::chrono_literals;


bool stop;     //停止信号
void sig_handler(int signum)
{
    if (signum == SIGINT || signum == SIGTERM) {
        stop = true;
    }
}

// //定义名字为name的参数默认值为Unknown
// this->declare_parameter<std::string>("name", "Unknown");
// //获取参数名字为name的值
// this->get_parameter("name", parameter_string_);
class MpuPublisher : public rclcpp::Node
{
public:
    MpuPublisher()
    : Node("mpu_publisher")
    {
        // //定义名字为name的参数默认值为Unknown
        // this->declare_parameter<int16_t>("offsetf",2048);    // 前角度传感器中间值
        // this->declare_parameter<int16_t>("offsetr",2048);    // 后角度中间值
        // //获取参数名字为name的值
        // this->get_parameter("offsetf", parameter_string_);
        // this->get_parameter("offsetr", parameter_string_);
        // RCLCPP_INFO(this->get_logger(), "name: %s", parameter_string_.c_str());

        /* 建立iio设备的连接 */
        publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("mpu6050", 10); 
        take_signals();                     // 接管信号
        stop = false;
    }

    void run(void)
    {
        ssize_t nbytes_rx;
        char *p_dat, *p_end;
        ptrdiff_t p_inc;

        if( iio_init() != true ){
            goto fail;
        }
        while(stop == false){
            nbytes_rx = iio_buffer_refill(rxbuf);
            if (nbytes_rx < 0) {
                printf("Error refilling buf: %d\n", (int)nbytes_rx);
                goto fail;
            }
            p_inc = iio_buffer_step(rxbuf);
            p_end = (char*)iio_buffer_end(rxbuf);
            for (p_dat = (char *)iio_buffer_first(rxbuf, accx); p_dat < p_end; p_dat += p_inc) {
                acc_data[0] = ((p_dat[0]<<8)|p_dat[1]);
                acc_data[1] = ((p_dat[2]<<8)|p_dat[3]);
                acc_data[2] = ((p_dat[4]<<8)|p_dat[5]);
                gyro_data[0] = (p_dat[6]<<8)|p_dat[7];
                gyro_data[1] = (p_dat[8]<<8)|p_dat[9];
                gyro_data[2] = (p_dat[10]<<8)|p_dat[11];
                timestamp = *((int64_t *)(&p_dat[16]));     // 获得 数据
                ros_publish();           // 发布数据
            }
        }
        /* 关闭设备 */
        fail:
        shutdown();
    }

private:
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    sensor_msgs::msg::Imu message;
    imu imu_p;   //处理数据
    /* 滤波器 */
    // Dsp::Filter *filter;   // 滤波器
    /* 读回来的数据 */
    int16_t acc_data[3] = {0};
    int16_t gyro_data[3] = {0};
    int64_t timestamp = {0};    //
    double acc_dobule[3] = {0};
    double gyro_double[3] = {0};

    double gyro_offset[3] = {0};   //偏移值  需要设置参数

    // int64_t last_timestamp;    //时间戳
    /* iio 设备*/
    struct iio_context *ctx = NULL;
    struct iio_device *mpu = NULL;

    struct iio_channel *accx = NULL;
    struct iio_channel *accy = NULL;
    struct iio_channel *accz = NULL;
    struct iio_channel *gyrox = NULL;
    struct iio_channel *gyroy = NULL;
    struct iio_channel *gyroz = NULL;
    struct iio_channel *time = NULL;

    struct iio_buffer  *rxbuf;    // 读取的buffer

    bool iio_init(void){
        ctx = iio_create_default_context();
        if(ctx == NULL) return false;
        if( iio_context_get_devices_count(ctx) <= 0 ){
            perror("No devices\n");
            return false;
        }
        /* 寻找设备 */
        mpu = iio_context_find_device(ctx, "mpu6050");
        if(mpu == NULL){
            perror("didn't find mpu6050\n");
            return false;
        }
        if( iio_device_attr_write_longlong(mpu,"sampling_frequency",500) < 0 ){
            printf("sampling_frequency error\n"); 
        }
        /* 读取数据 */
        accx = iio_device_find_channel(mpu, "accel_x", false);          //寻找输入通道
        if(accx == NULL) {perror("didn't find mpu6050 accx channel\n"); return false;}
        accy = iio_device_find_channel(mpu, "accel_y", false);          //寻找输入通道
        if(accy == NULL) {perror("didn't find mpu6050 accy channel\n"); return false;}
        accz = iio_device_find_channel(mpu, "accel_z", false);          //寻找输入通道
        if(accz == NULL) {perror("didn't find mpu6050 accz channel\n"); return false;}
        gyrox = iio_device_find_channel(mpu, "anglvel_x", false);     //寻找输入通道
        if(gyrox == NULL) {perror("didn't find mpu6050 gyrox channel\n"); return false;}
        gyroy = iio_device_find_channel(mpu, "anglvel_y", false);     //寻找输入通道
        if(gyroy == NULL) {perror("didn't find mpu6050 gyroy channel\n"); return false;}
        gyroz = iio_device_find_channel(mpu, "anglvel_z", false);     //寻找输入通道
        if(gyroz == NULL) {perror("didn't find mpu6050 gyroz channel\n"); return false;}
        time = iio_device_find_channel(mpu, "timestamp", false);     //寻找输入通道
        if(time == NULL){
            perror("didn't find mpu6050 timestamp channel\n"); 
            return false;
        }
        iio_channel_enable(accx);
        iio_channel_enable(accy);
        iio_channel_enable(accz);
        iio_channel_enable(gyrox);
        iio_channel_enable(gyroy);
        iio_channel_enable(gyroz);
        iio_channel_enable(time);

        /* 创建读取buffer */
        rxbuf = iio_device_create_buffer(mpu, 1, false);
        if ( !rxbuf ) {
            perror("Could not create buffer");
            return false;
        }
        return true;
    }

    /* 关闭设备 */
    void shutdown(void){
        if (rxbuf) iio_buffer_destroy(rxbuf);
        if (accx)  iio_channel_disable(accx);
        if (accy)  iio_channel_disable(accy);
        if (accz)  iio_channel_disable(accz);
        if (gyrox) iio_channel_disable(gyrox);
        if (gyroy) iio_channel_disable(gyroy);
        if (gyroz) iio_channel_disable(gyroz);
        if (time)  iio_channel_disable(time);
        if (time)  iio_channel_disable(time);
        if (ctx)   iio_context_destroy(ctx);
    }

    /* 接管强制退出信号 */
    int take_signals(void){
        struct sigaction sa;
        memset(&sa, 0, sizeof(sa));
        sigset_t mask;
        memset(&mask,0,sizeof(sigset_t));    //要手动清空

        sa.sa_handler = sig_handler;
        sigemptyset(&sa.sa_mask);
        sigemptyset(&mask);

        if (sigaction(SIGTERM, &sa, NULL) < 0) {
            printf("sigaction: %s\n", strerror(errno));
            return -1;
        }

        if (sigaction(SIGINT, &sa, NULL) < 0) {
            printf("sigaction: %s\n", strerror(errno));
            return -1;
        }

        sigaddset(&mask, SIGINT);
        sigaddset(&mask, SIGTERM);
        /* make sure these signals are unblocked */
        if (sigprocmask(SIG_UNBLOCK, &mask, NULL)) {
            printf("sigprocmask: %s", strerror(errno));
            return -1;
        }
        return 0;
    }

    void ros_publish(void){
        rclcpp::Time time(timestamp);             // 获取传感器读取时的时间戳
        //message.header.stamp = this->now();     // 时间戳
        message.header.stamp = time;
        message.header.frame_id = "base_link";
        // if(last_timestamp == 0){
        //     last_timestamp = timestamp;
        // }

        #define GYRO_COFF 0.0010652644360316951     //+-2000 dps  这个系数是转换到了弧度制     2000/32768*math.pi/180  国际单位制  rad/s
        gyro_double[0] = gyro_data[0]*GYRO_COFF - gyro_offset[0];
        gyro_double[1] = gyro_data[1]*GYRO_COFF - gyro_offset[0];
        gyro_double[2] = gyro_data[2]*GYRO_COFF - gyro_offset[0];
        message.angular_velocity.x = gyro_double[0];
        message.angular_velocity.y = gyro_double[1];
        message.angular_velocity.z = gyro_double[2];

        /* g=9.80665  */
        #define ACC_COFF 0.0005985504150390625    //+- 2g  ，系数为 2*g/32768       都是国际单位制   m/s2
        acc_dobule[0] = acc_data[0]*ACC_COFF;
        acc_dobule[1] = acc_data[1]*ACC_COFF;
        acc_dobule[2] = acc_data[2]*ACC_COFF;
        message.linear_acceleration.x = acc_dobule[0];
        message.linear_acceleration.y = acc_dobule[1];
        message.linear_acceleration.z = acc_dobule[2];

        // #define GYRO_SCALE (180/3.141592653589793)
        /* 计算四元数 */
        imu_p.process(gyro_double[0],gyro_double[1],gyro_double[2],acc_data[0],acc_data[1],acc_data[2],0.002f);
        // last_timestamp = timestamp;
        message.orientation.w = imu_p.getQ0();
        message.orientation.x = imu_p.getQ1();
        message.orientation.y = imu_p.getQ2();
        message.orientation.z = imu_p.getQ3();

        //RCLCPP_INFO(rclcpp::get_logger("adc"), "raw value is [%d %d]", voltage[0], voltage[1]);
        publisher_->publish(message);     //发布
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    // AdcAnglePublisher adc;
    auto pub_node = std::make_shared<MpuPublisher>();
    pub_node->run();     //  不断运行
    rclcpp::shutdown();
    return 0;
}



